package com.googlecode.curvedmetromaps.core.geometry;
//Geometric Tools, LLC
//Copyright (c) 1998-2013
//Distributed under the Boost Software License, Version 1.0.
//http://www.boost.org/LICENSE_1_0.txt
//http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
//
//File Version: 5.0.0 (2010/01/01)

public class BSplineCurveFit {

	private int				mDimension;
	private int				mNumSamples;
	private double[]		mSampleData;

	// The fitted B-spline curve, open and with uniform knots.
	private int				mDegree;
	private int				mNumControls;
	private double[]		mControlData;
	private BSplineFitBasis	mBasis;

	// ----------------------------------------------------------------------------
	public BSplineCurveFit(int dimension, int numSamples, double[] sampleData, int degree, int numControls) {
		mBasis = new BSplineFitBasis(numControls, degree);

		assert dimension >= 1 : "Invalid input\n";
		assert 1 <= degree && degree < numControls : "Invalid input\n";
		assert numControls <= numSamples : "Invalid input\n";

		mDimension = dimension;
		mNumSamples = numSamples;
		mSampleData = sampleData;
		mDegree = degree;
		mNumControls = numControls;
		mControlData = new double[mDimension * numControls];

		// The double-precision basis functions are used to help with the
		// numerical round-off errors.
		BSplineFitBasis dBasis = new BSplineFitBasis(mNumControls, mDegree);
		double tMultiplier = 1.0 / (double) (mNumSamples - 1);

		// Fit the data points with a B-spline curve using a least-squares error
		// metric. The problem is of the form A^T*A*Q = A^T*P, where A^T*A is a
		// banded matrix, P contains the sample data, and Q is the unknown
		// vector
		// of control points.

		double t;
		int i0, i1, i2, imin = 0, imax = 0, j;

		// Construct the matrix A^T*A.
		BandedMatrix ATAMat = new BandedMatrix(mNumControls, mDegree + 1, mDegree + 1);

		for (i0 = 0; i0 < mNumControls; ++i0) {
			for (i1 = 0; i1 < i0; ++i1) {
				ATAMat.set(i0, i1, ATAMat.get(i1, i0));
			}

			int i1Max = i0 + mDegree;
			if (i1Max >= mNumControls) {
				i1Max = mNumControls - 1;
			}

			for (i1 = i0; i1 <= i1Max; ++i1) {
				double value = 0.0;
				for (i2 = 0; i2 < mNumSamples; ++i2) {
					t = tMultiplier * (double) i2;

					// Java does not support arguments by reference
					int[] minMax = { imin, imax };
					dBasis.Compute(t, minMax);
					imin = minMax[0];
					imax = minMax[1];

					if (imin <= i0 && i0 <= imax && imin <= i1 && i1 <= imax) {
						double dB0 = dBasis.GetValue(i0 - imin);
						double dB1 = dBasis.GetValue(i1 - imin);
						value += dB0 * dB1;
					}
				}
				ATAMat.set(i0, i1, value);
			}
		}

		// Construct the matrix A^T.
		double[][] ATMat = new double[mNumControls][mNumSamples];
		for (i0 = 0; i0 < mNumControls; ++i0) {
			for (i1 = 0; i1 < mNumSamples; ++i1) {
				t = tMultiplier * (double) i1;

				// Simulate passing by reference
				int[] minMax = { imin, imax };
				dBasis.Compute(t, minMax);
				imin = minMax[0];
				imax = minMax[1];

				if (imin <= i0 && i0 <= imax) {
					ATMat[i0][i1] = dBasis.GetValue(i0 - imin);
				}
			}
		}

		// Compute X0 = (A^T*A)^{-1}*A^T by solving the linear system
		// A^T*A*X = A^T.
		boolean solved = ATAMat.SolveSystem(ATMat, mNumSamples);
		assert solved : "Failed to solve linear system";

		// The control points for the fitted curve are stored in the vector
		// Q = X0*P, where P is the vector of sample data.
		for (i0 = 0; i0 < mNumControls; ++i0) {
			int Qi = i0 * mDimension;
			for (i1 = 0; i1 < mNumSamples; ++i1) {
				int Pi = i1 * mDimension;
				double xValue = ATMat[i0][i1];
				for (j = 0; j < mDimension; j++) {
					mControlData[Qi + j] += xValue * mSampleData[Pi + j];
				}
			}
		}

		// Set the first and last output control points to match the first and
		// last input samples. This supports the application of fitting keyframe
		// data with B-spline curves. The user expects that the curve passes
		// through the first and last positions in order to support matching two
		// consecutive keyframe sequences.
		int ci = mDimension * (mNumControls - 1);
		int si = mDimension * (mNumSamples - 1);
		for (j = 0; j < mDimension; j++) {
			mControlData[j] = mSampleData[j];
			mControlData[ci + j] = mSampleData[si + j];
		}
	}

	// ----------------------------------------------------------------------------
	public double[] GetPosition(double t) {
		double[] position = new double[mDimension];

		int[] minMax = new int[2];
		mBasis.Compute(t, minMax);
		int imin = minMax[0];
		int imax = minMax[1];

		int sIndex = mDimension * imin;
		double basisValue = mBasis.GetValue(0);
		int j;
		for (j = 0; j < mDimension; ++j) {
			position[j] = basisValue * mControlData[sIndex++];
		}

		for (int i = imin + 1, index = 1; i <= imax; ++i, ++index) {
			basisValue = mBasis.GetValue(index);
			for (j = 0; j < mDimension; ++j) {
				position[j] += basisValue * mControlData[sIndex++];
			}
		}

		return position;
	}

}
